#include "pid.h"
#include "lcd/lcd.h"
#include "stdio.h"
#include "utils/debug_uart.h"

void pid_init(PID *pid, float p, float i, float d, float max_dynamic_i, float max_output, int target)
{
    pid->kp = p;
    pid->ki = i;
    pid->kd = d;
    pid->max_dynamic_i = max_dynamic_i;
    pid->max_output = max_output;
    pid->target = target;

    pid->dynamic_p = 0;
    pid->dynamic_i = 0;
    pid->dynamic_d = 0;
}

void pid_dynamic_clear(PID *pid)
{
    pid->dynamic_p = 0;
    pid->dynamic_i = 0;
    pid->dynamic_d = 0;
}

float pid_calc(PID *pid, float target, float current)
{
    // 用上一次的误差值更新 之前误差last_error
    pid->last_error = pid->error;
    // 获取新的误差 = 目标值 - 当前值
    pid->error = target - current;


     //计算微分
    float dout = (pid->error - pid->last_error) * pid->kd;
    //计算比例
    float pout = pid->error * pid->kp;
    //计算积分
    pid->dynamic_i += pid->error * pid->ki;

    //积分限幅
    if(pid->dynamic_i > pid->max_dynamic_i) pid->dynamic_i = pid->max_dynamic_i;
    else if(pid->dynamic_i < -pid->max_dynamic_i) pid->dynamic_i = -pid->max_dynamic_i;

    //计算输出
    pid->output = pout + dout + pid->dynamic_i;	

    //输出限幅
    if(pid->output > pid->max_output) pid->output =   pid->max_output;
    else if(pid->output < -pid->max_output) pid->output = -pid->max_output;

    // 返回PID计算的结果
    return pid->output;
}